package geom;
import gnu.io.SerialPortEvent;

import java.util.BitSet;


import javax.media.j3d.*;
import javax.vecmath.Point3d;

import com.sun.j3d.utils.behaviors.picking.Intersect;
import processing.core.*;
import util.BitArray;

public class Geom implements PConstants{
	public static float TWO_PI = 2 * PI;
	
	public static float normalizeAngle(float a){
		while(a < 0) a += TWO_PI;
		while(a >= TWO_PI) a -= TWO_PI;
		return a;
	}
	
	public static boolean containsAngle(float sAngle, float eAngle, float angle){
		sAngle = normalizeAngle(sAngle);
		eAngle = normalizeAngle(eAngle);
		angle = normalizeAngle(angle);
		if(sAngle <= eAngle) return angle > sAngle && angle < eAngle;
		else{
			return (angle > sAngle && angle <= TWO_PI) ||
				   (angle > 0 && angle <= eAngle);
		}
	}
	
	public static float getShortestAngleDistance(float a1, float a2){
		a1 = normalizeAngle(a1);
		a2 = normalizeAngle(a2);
		float a = a2 - a1;
		if(a > PI) a = TWO_PI - a;
		if(a < -PI) a = -TWO_PI - a;
		return a;
	}
	
	public static float distance(Vector vector1, Vector vector2) {
		return distance(vector1.x, vector1.y, vector1.z, vector2.x, vector2.y, vector2.z);
	}
	
	public static float distance(float x1, float y1, float x2, float y2){
		return PApplet.sqrt(PApplet.sq(x2 - x1) + PApplet.sq(y2 - y1));
	}
	
	public static float distance(float x1, float y1, float z1, float x2, float y2, float z2){
		return PApplet.sqrt(PApplet.sq(x2 - x1) + PApplet.sq(y2 - y1) + PApplet.sq(z2 - z1));
		
	}
	
	public static float sqrDistance(Vector vector1, Vector vector2) {
		return sqrDistance(vector1.x, vector1.y, vector1.z, vector2.x, vector2.y, vector2.z);
	}
	public static float sqrDistance(float x1, float y1, float x2, float y2){
		return PApplet.sqrt(PApplet.sq(x2 - x1) + PApplet.sq(y2 - y1));
	}
	
	public static float sqrDistance(float x1, float y1, float z1, float x2, float y2, float z2){
		return PApplet.sqrt(PApplet.sq(x2 - x1) + PApplet.sq(y2 - y1) + PApplet.sq(z2 - z1));
		
	}
	
	public static float angle(float x1, float y1){
		return angle(0, 0, x1, y1);
	}
	
	public static float angleXY(Vector v1, Vector v2){
		return angle(v1.x, v1.y, v2.x, v2.y);
	}
	
	public static float angle(float x1, float y1, float x2, float y2){
		float dx = x2 - x1;
		if(dx == 0) dx = 0.0001f;
		float a = (float)Math.atan((y2 - y1)/dx);
		if(dx < 0) a -= PI;
		return a;
	}
	
	public static float interpolate(float x1, float x2, float t){
		return interpolate(x1, x2, t, 1);
	}
	
	public static float interpolateLin(float x1, float x2, float t, int pow){
		return x1 * (1 - t) + x2 * t;
		
	}
	public static float interpolate(float x1, float x2, float t, float pow){
		if(pow < 0) pow = -1/pow;
		return x1 * (1 - PApplet.pow(t, pow)) + x2 * PApplet.pow(t, pow);
		
	}
	/*
	public static Circle interpolateCircle(Circle circle1, Circle circle2, float t){
		return new Circle(interpolate(circle1.x, circle2.x, t), 
						 interpolate(circle1.y, circle2.y, t), 
						 interpolate(circle1.r, circle2.r, t));
	}
	*/
	public static float[] add(float[] a, float[] b){
		if(a.length != b.length) return null;
		float[] c = new float[a.length];
		for(int i = 0; i < a.length; i++)
			c[i] = a[i] + b[i];
		return c;
	}
	
	public static float[] sub(float[] a, float[] b){
		if(a.length != b.length) return null;
		float[] c = new float[a.length];
		for(int i = 0; i < a.length; i++)
			c[i] = a[i] - b[i];
		return c;
	}
	
	public static float[] mult(float[] a, float b){
		float[] c = new float[a.length];
		for(int i = 0; i < a.length; i++)
			c[i] = a[i] * b;
		return c;
	}
	
	public static float[] div(float[] a, float b){
		float[] c = new float[a.length];
		for(int i = 0; i < a.length; i++)
			c[i] = a[i] * b;
		return c;
	}


	
	public static float getAngleSumDiff(Vector v1, Vector v2, Vector v3){
		PolarVector angleVector1 = v2.sub(v1).getPolar();
		PolarVector angleVector2 = v3.sub(v2).getPolar();
		float angle1 = PApplet.sqrt(angleVector1.a * angleVector1.a + angleVector1.e * angleVector1.e);
		float angle2 = PApplet.sqrt(angleVector2.a * angleVector2.a + angleVector2.e * angleVector2.e);
		return PApplet.min(PApplet.abs(angle2 - angle1), 2*PI - PApplet.abs(angle2 - angle1));
	}

	public static float getAngleSumDiff2D(Vector v1, Vector v2, Vector v3){
		PolarVector angleVector1 = v2.sub(v1).getPolar();
		PolarVector angleVector2 = v3.sub(v2).getPolar();
		return PApplet.min(PApplet.abs(angleVector2.a - angleVector1.a), 2*PI - PApplet.abs(angleVector2.a - angleVector1.a));
	}
	
	public static float getAngleSum(float az, float el, float ro){
		return 2 * 	(float)Math.acos(	
						(float)Math.cos(az) * 
						(float)Math.cos(el) * 
						(float)Math.cos(ro) - 
						(float)Math.sin(az) * 
						(float)Math.sin(el) * 
						(float)Math.sin(ro)	
					);
	}
	
	public static AxisAngle eulerToAxisAngle(Vector v){
		PolarVector pv = v.getPolar();
		return eulerToAxisAngle(pv.a, pv.e, 0);
	}
	
	public static AxisAngle eulerToAxisAngle(float az, float el, float ro){
		az = az / 2f;
		el = el / 2f;
		ro = ro / 2f;
		float c1 = (float)Math.cos(az);
		float c2 = (float)Math.cos(el);
		float c3 = (float)Math.cos(ro);
		float s1 = (float)Math.sin(az);
		float s2 = (float)Math.sin(el);
		float s3 = (float)Math.sin(ro);
		//System.out.println("c1:" + c1 + " c2 " + c2 + " c3 " + c3 + " s1 " + s1 + " s2 " + s2  + " s3 " + s3);
		AxisAngle aa = new AxisAngle();
		aa.setX(s1 * s2 * c3 + c1 * c2 * s3);
		aa.setY(s1 * c2 * c3 + c1 * s2 * s3);
		aa.setZ(c1 * s2 * c3 - s1 * c2 * s3);
		//System.out.println("acos: " + (c1 * c2 * c3 - s1 * s2 * s3));
		aa.setA(2 * (float)Math.acos(c1 * c2 * c3 - s1 * s2 * s3));
		return aa;
	}
	

	
	public static String printInts(int[] b){
		StringBuffer sb = new StringBuffer();
		for(int i = 0; i < b.length; i++){
			sb.append(b[i]);
			sb.append(", ");
		}
		return sb.toString();
	}
	
	public static String printBytes(byte[] b){
		StringBuffer sb = new StringBuffer();
		for(int i = 0; i < b.length; i++){
			sb.append(b[i]);
			sb.append(", ");
		}
		return sb.toString();
	}
	
	public static String printBits(byte[] b){
		StringBuffer sb = new StringBuffer();
		sb.append("Bits: ");
		for(int i = 0; i < b.length; i++){
			sb.append("byte ");
			sb.append(i);
			sb.append(": ");
			BitArray bits = new BitArray(b[i], 8);
			sb.append(bits.toString());
			sb.append(" ");
		}
		return sb.toString();
	}
	
	public static String printBits(byte b){
		return printBits(new byte[]{b});
	}
	
    // Returns a bitset containing the values in bytes.
    // The byte-ordering of bytes must be big-endian which means the most significant bit is in element 0.
    public static BitSet toBitSet(byte[] bytes) {
        BitSet bits = new BitSet(8);
        for (int i=0; i<bytes.length*8; i++) {
            if ((bytes[bytes.length-i/8-1]&(1<<(i%8))) > 0) {
                bits.set(i);
            }
        }
        return bits;
    }
    
    public static BitSet toBitSet(byte b){
    		return toBitSet(new byte[]{b});
    }
	
	public String printSerialEvent(SerialPortEvent e){
		StringBuffer sb = new StringBuffer();
		sb.append("SerialEvent Recieved. EventType: ");
		int eventType = e.getEventType();
		switch(eventType){
		case SerialPortEvent.BI: 					sb.append("BreakInterrupt "); break;
		case SerialPortEvent.CD: 					sb.append("CarrierDetect "); break;
		case SerialPortEvent.CTS: 				sb.append("ClearToSend "); break;
		case SerialPortEvent.DATA_AVAILABLE: 		sb.append("DataAvailable "); break;
		case SerialPortEvent.DSR: 				sb.append("DataSetReady "); break;
		case SerialPortEvent.FE: 					sb.append("FramingError "); break;
		case SerialPortEvent.OE: 					sb.append("OverrunError "); break;
		case SerialPortEvent.OUTPUT_BUFFER_EMPTY: 	sb.append("OutputBufferEmpty "); break;
		case SerialPortEvent.PE: 					sb.append("ParityError "); break;
		case SerialPortEvent.RI: 					sb.append("RingIndicator"); break;
		}
		if(e.getNewValue()) sb.append("true");
		else sb.append("false");
		return sb.toString();
	}
	
	public static void orientToPath(PApplet pa, Vector v1, Vector v2, Vector c){
		orientToPath(pa, v1, v1, v2, c);
	}
	
	public static void orientToPath(PApplet pa, Vector v0, Vector v1, Vector v2, Vector c){
		PMatrix matrix = new PMatrix(2);
		Vector tangent = v2.sub(v0);
		Vector perp = v1.sub(c).xProdut(tangent).normalize();
		
		matrix.translate(v1.x, v1.y, v1.z);
		PolarVector pTan = tangent.getPolar();
		matrix.rotateZ(pTan.a);
		matrix.rotateY(pTan.e);
		
		PMatrix invMatrix = new PMatrix(2);
		invMatrix.invApply(matrix);
		float[] perpArray = perp.add(v1).getArray();
		invMatrix.mult3(perpArray, perpArray);
		perp = new Vector(perpArray);
		if(perp.y == 0) perp.y = 0.0001f;
		float perpAngle = pa.atan(perp.z / perp.y);
		if(perp.z > 0) perpAngle += PI;
		matrix.rotateX(perpAngle);
		pa.applyMatrix(	matrix.m00, matrix.m01, matrix.m02, matrix.m03,
					  	matrix.m10, matrix.m11, matrix.m12, matrix.m13,
					  	matrix.m20, matrix.m21, matrix.m22, matrix.m23,
					  	matrix.m30, matrix.m31, matrix.m32, matrix.m33);
	}
	
	public float determinant(Vector a, Vector b, Vector c){
		return 	a.x * b.y * c.z - 
				a.x * b.z * c.y - 
				a.y * b.x * c.z + 
				a.y * b.z * c.x +
				a.z * b.x * b.z -
				a.z * b.y * c.x;
	}

	
	public static boolean intersect(Vector a1, Vector a2, Vector b1, Vector b2, double[] dist){
		return Intersect.segmentAndLine(
				new PickSegment(new Point3d(a1.x, a1.y, a1.z), new Point3d(a2.x, a2.y, a2.z)),
				new Point3d[]{new Point3d(b1.x, b1.y, b1.z), new Point3d(b2.x, b2.y, b2.z)}, 0, dist);
	}
	
	
	/*
	public static float[] intersectLineAndPlane(Vector p0, Vector p1, Vector p2, Vector la, Vector lb){
		double[][] array = {	{la.x - lb.x, p1.x - p0.x, p2.x - p0.x},
							{la.y - lb.y, p1.y - p0.y, p2.y - p0.y},
							{la.z - lb.z, p1.z - p0.z, p2.z - p0.z} }; 
		Matrix A = new Matrix(array);
		if(!(new LUDecomposition(A).isNonsingular())) return null;
		double[][] array2 = {	{la.x - p0.x}, {la.y - p0.y}, {la.z - p0.z}};
		Matrix B = new Matrix(array2);
		Matrix x = A.solve(B);
		return new float[]{ (float) x.get(0, 0),  (float) x.get(1, 0), (float) x.get(2, 0)};
	}
	*/
	public static float[] quadraticEquation(float a, float b, float c){
		float v = (float) Math.sqrt(Math.pow(b, 2) - 4 * a * c);
		return new float[]{
				(b - v) / (2 * a),
				(b + v) / (2 * a)
		};
	}
	
	public static Vector intersectLineSphere(Vector l1, Vector l2, Vector s, float r){
		float i = l2.x - l1.x;
		float j = l2.y - l1.y;
		float k = l2.z - l1.z;
		float a = (float)(Math.pow(i, 2) + Math.pow(j, 2) + Math.pow(k, 2));
		float b = 2 * i * (l1.x - s.x) + 2 * j * (l1.y - s.y) + 2 * k * (l1.z - s.z);
		float c = (float)(Math.pow(l1.x - s.x, 2) + Math.pow(l1.y - s.y, 2) + Math.pow(l1.z - s.z, 2) - Math.pow(r, 2));
		float[] sol = quadraticEquation(a, b, c);
		int sn = -1;
		System.out.println("sol: " + sol[0] + ", " + sol[1]);
		if(sol[0] >= 0 && sol[0] <= 1) sn = 0;
		else if(sol[1] >= 0 && sol[1] <= 1) sn = 1;
		if(sn == -1) return null;
		
		return new Vector(l1.x + sol[sn] * i, l1.y + sol[sn]* j, l1.z + sol[sn] * k);
		
		
	}
	//returns true if a line intersects another line
	
	public static final boolean linesIntersectXY(Vector p1, Vector p2, Vector p3, Vector p4)
		{
		// From Graphics Gems II
		float r, s, rsDenom, rNumer;
		float dp1yp3y, dp4xp3x, dp1xp3x, dp4yp3y, dp2xp1x, dp2yp1y;		
		
		dp1yp3y = p1.y - p3.y;
		dp4xp3x = p4.x - p3.x;
		dp1xp3x = p1.x - p3.x;
		dp4yp3y = p4.y - p3.y;
		
		dp2xp1x = p2.x - p1.x;
		dp2yp1y = p2.y - p1.y;
		
		rsDenom = dp2xp1x * dp4yp3y - dp2yp1y * dp4xp3x;
		rNumer = (dp1yp3y * dp4xp3x - dp1xp3x * dp4yp3y);
		
			// if rsDenom is zero, lines are parallel
			// if rNumer is also zero, lines are collinear
		if (rsDenom == 0)
			if (rNumer != 0)
				return false;
			else
				{
				// Check for overlap
				float hix1, hiy1, lox1, loy1;
				if (p1.x > p2.x)
					{
					hix1 = p1.x;
					lox1 = p2.x;
					}
				else
					{
					hix1 = p2.x;
					lox1 = p1.x;
					}

				if (p1.y > p2.y)
					{
					hiy1 = p1.y;
					loy1 = p2.y;
					}
				else
					{
					hiy1 = p2.y;
					loy1 = p1.y;
					}

					// Check intervals
				return !(((lox1 > p3.x && lox1 > p4.x) || (hix1 < p3.x && hix1 < p4.x)) ||
					 ((loy1 > p3.y && loy1 > p4.y) || (hiy1 < p3.y && hiy1 < p4.y)));
				}
				
		r = rNumer / rsDenom;
		s = (dp1yp3y * dp2xp1x - dp1xp3x * dp2yp1y) / rsDenom;
		
		return !(r < 0 || r > 1 || s < 0 || s > 1);
    }
	
	public static Vector getXY(float a, float r){
		return new Vector(r * (float)Math.cos(a), r * (float)Math.sin(a)); }
    
		/**
		 * Returns the point at which two line segments intersect, or null if they don't.
		 * If the two lines are collinear and intersect, the intersection point returned
		 * is the one closest to p1.
		 * @return the point at which the two segments intersect.
		 */
	public static final Vector linesIntersectAtXY(Vector p1, Vector p2, Vector p3, Vector p4){ return linesIntersectAtXY(p1, p2, p3, p4, null); }
	public static final Vector linesIntersectAtXY(Vector p1, Vector p2, Vector p3, Vector p4, Vector intersect){
		// From Graphics Gems II
		float r, s, rsDenom, rNumer;
		float dp1yp3y, dp4xp3x, dp1xp3x, dp4yp3y, dp2xp1x, dp2yp1y;		

		if (intersect == null) intersect = new Vector();
		
		dp1yp3y = p1.y - p3.y; dp4xp3x = p4.x - p3.x;
		dp1xp3x = p1.x - p3.x; dp4yp3y = p4.y - p3.y;
		dp2xp1x = p2.x - p1.x; dp2yp1y = p2.y - p1.y;
		
		rsDenom = dp2xp1x * dp4yp3y - dp2yp1y * dp4xp3x;
		rNumer = (dp1yp3y * dp4xp3x - dp1xp3x * dp4yp3y);
		
			// if rsDenom is zero, lines are parallel
			// if rNumer is also zero, lines are collinear
		if (rsDenom == 0)
			if (rNumer != 0) return null;
			else{ 
				// Check for overlap
				float hix1, hiy1, lox1, loy1;
				if (p1.x > p2.x) { hix1 = p1.x; lox1 = p2.x; }
				else{ hix1 = p2.x; lox1 = p1.x; }

				if (p1.y > p2.y) { hiy1 = p1.y; loy1 = p2.y; }
				else { hiy1 = p2.y; loy1 = p1.y; }

					// Check intervals
				if (((lox1 > p3.x && lox1 > p4.x) || (hix1 < p3.x && hix1 < p4.x)) ||
					 ((loy1 > p3.y && loy1 > p4.y) || (hiy1 < p3.y && hiy1 < p4.y)))
					return null;
				else{
					float d13, d14;
					float dp1xp4x, dp1yp4y;
						// Find distance p1-p3 and p1-p4 and choose closest, but
						// if one is negative and the other positive, then p1 lies between
						// so p1 is the intersecting point we want.
					dp1xp4x = p1.x - p4.x;
					if (dp1xp3x * dp1xp4x <= 0){
						intersect.set(p1); //copyFrom
						return intersect;
					}
						
					dp1yp4y = p1.y - p4.y;
					if (dp1yp3y * dp1yp4y <= 0){
						intersect.set(p1); //setFrom
						return intersect;
					}
						
					d13 = dp1xp3x * dp1xp3x + dp1yp3y * dp1yp3y;
					d14 = dp1xp4x * dp1xp4x + dp1yp4y * dp1yp4y;
					if (d13 < d14){
						intersect.set(p3); //copyFrom
						return intersect;
					}
					else{
						intersect.set(p4); //setFrom
						return intersect;
					}
				}
			}
				
		r = rNumer / rsDenom;		
		s = (dp1yp3y * dp2xp1x - dp1xp3x * dp2yp1y) / rsDenom;
		
		if (r < 0 || r > 1 || s < 0 || s > 1)
			return null;
		else{
			intersect.setXYZ(p1.x + r * (p2.x - p1.x), p1.y + r * (p2.y - p1.y));
			return intersect;
		}
    }
    

		
		/**
  	 * Returns the point on the specified line that is nearest to 
  	 * the specified target point.
  	 *
  	 * @param p1  the first point of the line.
  	 * @param p2  the second point of the line.
  	 * @param target  the target point.
  	 * @return the nearest point on the line.
  	 */
	public static Vector getNearestPointOnLineXY(Vector p1, Vector p2, Vector target){ return getNearestPointOnLineXY(p1, p2, target, null); }
	public static Vector getNearestPointOnLineXY(Vector p1, Vector p2, Vector target, Vector nearest){
		float dp2p1x, dp2p1y, dpp1x, dpp1y, r, sql12;
		
		if (nearest == null) nearest = new Vector();
		if (target == null) System.out.println("vvvv===NULL");
		dp2p1x = (p2.x - p1.x); 
		dp2p1y = (p2.y - p1.y);
		dpp1x = (target.x - p1.x); 
		dpp1y = (target.y - p1.y);
		sql12 = ((dp2p1x * dp2p1x) + (dp2p1y * dp2p1y));
		r =  (dpp1x * dp2p1x + dpp1y * dp2p1y) / sql12;
		
		if (r <= 0) { nearest.x = p1.x; nearest.y = p1.y; }
		else { 
			if (r >= 1) {
				nearest.x = p2.x; nearest.y = p2.y; }
			else {
				float nx, ny;
				nx = p1.x + r * dp2p1x; ny = p1.y + r * dp2p1y;
				nearest.x = nx; nearest.y = ny;
			}
		}
		return nearest;
  	}    	
  	
  	/**
  	 * Returns the distance from the specified target point to 
  	 * the nearest point on the specified line.
  	 *
  	 * @param p1  the first point of the line.
  	 * @param p2  the second point of the line.
  	 * @param target  the target point.
  	 * @return the computed distance.
  	 */
	public static float getDistanceFromPointToLineXY(Vector p1, Vector p2, Vector target){
		return getDistanceFromPointToLineXY(p1, p2, target, null);
  	}
  	
	public static float getDistanceFromPointToLineXY(Vector p1, Vector p2, Vector target, Vector nearest){
		nearest = getNearestPointOnLineXY(p1, p2, target);
		return target.getDistance(nearest);
  	}

}
